/*
 * @Description: some tools
 * @Autor: Zijie Chen
 * @Date: 2023-12-27 23:43:58
 */

#pragma once

#include <vector>

#include <Eigen/Core>

//计算一组数据的均值（mean）和对角线协方差（covariance diagonal）
template <typename C, typename D, typename Getter>
void ComputeMeanAndCovDiag(const C& data,   //输入数据，数据集
                           D& mean,         //用于存储计算得到的均值
                           D& cov_diag,     //用于存储计算得到的协方差对角线
                           Getter&& getter) //用于提取数据元素值的可调用对象
                           {
  size_t len = data.size();
  assert(len > 1);
    //使用 std::accumulate 来计算均值
    mean = std::accumulate(data.begin(), data.end(), D::Zero().eval(),
                           [&getter](const D& sum, const auto& data) -> D { return sum + getter(data); }) / len;
    //std::accumulate 来计算协方差的对角线
    cov_diag = std::accumulate(data.begin(), data.end(), D::Zero().eval(),
                               [&mean, &getter](const D& sum, const auto& data) -> D {
                                   return sum + (getter(data) - mean).cwiseAbs2().eval();
                               }) / (len - 1);
  // clang-format on
}
//通过给定的一组三维点估计一个平面，并返回该平面是否符合给定的阈值条件。代码采用了主成分分析（PCA）的思想来进行平面拟合。
inline bool EstimatePlane(Eigen::Vector4d& pca_result,
                          const std::vector<Eigen::Vector3d>& point,
                          const double threshold = 0.1) {
  //如果输入的点数小于3，则无法确定一个平面，函数返回false
  if (point.size() < 3) {
    return false;
  }

  Eigen::MatrixXd A(point.size(), 3);
  Eigen::VectorXd b(point.size(), 1);

  A.setZero();
  b.setOnes();
  b *= -1.0;

  //将每个点的x、y和z坐标填充到矩阵A中
  for (size_t j = 0; j < point.size(); j++) {
    A(j, 0) = point[j].x();
    A(j, 1) = point[j].y();
    A(j, 2) = point[j].z();
  }

  //通过最小二乘法解线性方程组Ax = b，得到平面的法向量normvec
  Eigen::Vector3d normvec = A.colPivHouseholderQr().solve(b);

  //计算法向量的模并将其归一化，使其成为单位向量，同时将结果存入pca_result中
  double n_norm = normvec.norm();
  pca_result(0) = normvec(0) / n_norm;
  pca_result(1) = normvec(1) / n_norm;
  pca_result(2) = normvec(2) / n_norm;
  pca_result(3) = 1.0 / n_norm;

  //对于每个输入点p，构造一个四维向量temp并检查它与平面参数pca_result的点积是否超过阈值。如果点积的绝对值超过阈值，则返回false
  for (const auto& p : point) {
    Eigen::Vector4d temp(p.x(), p.y(), p.z(), 1.0);
    if (fabs(pca_result.dot(temp)) > threshold) {
      return false;
    }
  }
  return true;
}

// CauchyLossFunction 的内联函数，主要用于计算基于Cauchy损失函数的值。这个函数通常在鲁棒统计和机器学习中用于处理数据异常值
inline void CauchyLossFunction(const double e,
                               const double delta,
                               Eigen::Vector3d& rho) {

  //计算 delta 的平方，将其保存在 dsqr 变量中，以便后续使用
  double dsqr = delta * delta;
  //通过判断误差 e 是否小于或等于 dsqr，来确定当前误差是一个“内点”（inlier）还是“外点”（outlier）,设置损失值\设置一阶导数\设置二阶导数
  if (e <= dsqr) {  // inlier:
    rho[0] = e;
    rho[1] = 1.;
    rho[2] = 0.;
  } else {                   // outlier
    double sqrte = sqrt(e);  // absolut value of the error
    // rho(e)   = 2 * delta * e^(1/2) - delta^2
    rho[0] = 2 * sqrte * delta - dsqr;
    // rho'(e)  = delta / sqrt(e)
    rho[1] = delta / sqrte;
    // rho''(e) = -1 / (2*e^(3/2)) = -1/2 * (delta/e) / e
    rho[2] = -0.5 * rho[1] / e;
  }
}